/***************************************************************************************************************************
* set_mode.cpp
*
* Author: SFL
*
* Update Time: 2019.3.16
*
* Introduction:  Change mode
                单独运行用来测试锁定解锁，设置模式之类的是否正确.
***************************************************************************************************************************/

//头文件
#include <ros/ros.h>


#include <iostream>

//话题头文件
#include <usart_to_sbus.h>
#include <command_to_drone.h>



using namespace std;

// uint8_t radioMsg[32]={0x0a, 0x0a ,0x13 ,0x00 ,0x25 ,0x00, 0x34 ,0x11 ,0x44 ,0x11, 0x55 ,0xdc, 0x65 ,0xdc,0x75 ,0xdc,0x85 ,0xdc, 0x95 ,0xdc, 0xa4 ,0x11 ,0xb4 ,0x11, 0xc4, 0x11 ,0xd4 ,0x11 ,0xe4 ,0x11 ,0xf4, 0x11};


//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "set_mode");
    ros::NodeHandle nh("~");
    command_to_drone _command_to_drone;
    bool success = setUpSBusSerialPort("/dev/ttyUSB0");
    if( !success )
        throw std::runtime_error( "Unable to open UART port" );
    int len = 0;

    ros::Rate rate(100);

    int Num_StateMachine = 0;
    int flag_1;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        switch (Num_StateMachine)
        {
            // input
            case 0:
                cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>--------<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
                cout << "Input the mode:  0 for ACRO, 1 for ANGLE, 2 for HORIZON, 3 for disarm, 4 for arm "<<endl;
                cin >> flag_1;

                //1000 降落 也可以指向其他任务
                if (flag_1 == 0)
                {
                    Num_StateMachine = 1;
                    break;
                }
                else if (flag_1 == 1)
                {
                    Num_StateMachine = 2;
                    break;
                }
                else if(flag_1 == 2)
                {
                    Num_StateMachine = 3;
                }//惯性系移动
                else if(flag_1 == 3)
                {
                    Num_StateMachine = 4;
                }
                else if(flag_1 == 4)
                {
                    Num_StateMachine = 5;
                }

                break;

        //ACRO
        case 1:
            Num_StateMachine = 0;
            break;

        //STABILIZED
        case 2:
            Num_StateMachine = 0;
            break;

        //HORIZON
        case 3:
            Num_StateMachine = 0;
            break;

        //disarm
        case 4:
            _command_to_drone.arm_disarm(false);
            Num_StateMachine = 0;
            break;

        //arm
        case 5:
            _command_to_drone.arm_disarm(true);
            Num_StateMachine = 0;
            break;

        }

        len = write(serial_port_fd_,radioMsg,sizeof(radioMsg));
        rate.sleep();
    }

    return 0;
}
